package edu.wpi.first.wpilibj.defaultCode;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;

class PIDDebugUtil
{
    public static void enterPIDDebug(PIDController controller, Joystick joystick, String name)
    {
       int selectionMade = -1;
       double currValue = 0;

       System.out.println("\nEntering PID Maintenance mode for " + name);

       System.out.println("To modify P, press button 7.");
       System.out.println("To modify I, press button 9.");
       System.out.println("To modify D, press button 11.");
       System.out.println("To exit this mode, press the trigger.");

       while(selectionMade == -1)
       {
           if(joystick.getRawButton(7))
           {
               System.out.println("Modifying P.");
               currValue = controller.getP();
               selectionMade = 7;
           }
           if(joystick.getRawButton(9))
           {
               System.out.println("Modifying I.");
               currValue = controller.getI();
               selectionMade = 9;
           }
           if(joystick.getRawButton(11))
           {
               System.out.println("Modifying D.");
               currValue = controller.getD();
               selectionMade = 10;
           }
           if(joystick.getRawButton(1))
           {
               System.out.println("Exiting maintenance mode.");
               selectionMade = 1;
               return;
           }
       }

       System.out.println("Current value is: " + currValue);
       System.out.println("To increase, press forward. To decrease, pull back on the joystick.");
       System.out.println("To select the new value, press the trigger.");

       double yValue;
       while(!joystick.getRawButton(1))
       {
           yValue = joystick.getY();

           if(Math.abs(yValue) >=.2)
           {
               if(yValue <= .2)
               {
                   currValue = currValue + .001;
               }
               else
               {
                   currValue = currValue - .001;
               }

               System.out.println(currValue);

               try
               {
                Thread.sleep(500);
               }
               catch (InterruptedException ex)
                {
                    ex.printStackTrace();
                }
           }
       }

       switch(selectionMade)
       {
           case 7:
               setNewP(controller, currValue);
               break;
           case 9:
               setNewI(controller, currValue);
               break;
           case 11:
               setNewD(controller, currValue);
               break;
       }

       System.out.println("End of maintenance mode.");
    }

        private static void setNewP(PIDController controller, double p)
        {
            controller.setPID(p, controller.getI(), controller.getD());
            System.out.println("Now set to: " + controller.getP() + " " + controller.getI() + " " + controller.getD());
        }

        private static void setNewI(PIDController controller, double i)
        {
            controller.setPID(controller.getP(), i, controller.getD());
        }

        private static void setNewD(PIDController controller, double d)
        {
            controller.setPID(controller.getP(), controller.getI(), d);
        }
}